#include <Servo.h>

#define N_HEAD 1
#define I_HEAD_NECK 0

Servo servo_head[N_HEAD];
int ns_head[N_HEAD] = {
  14};
int pos_head[N_HEAD] = {
  90};

#define N_ARM 3
#define I_ARM_SHOULDER 0
#define I_ARM_BICEP 1
#define I_ARM_ELBOW 2

Servo servo_left_arm[N_ARM];
int ns_left_arm[N_ARM] = {
  33, 34, 35};
int pos_left_arm[N_ARM] = {
  90-60, 180, 180};

Servo servo_right_arm[N_ARM];
int ns_right_arm[N_ARM] = {
  37, 38, 39};
int pos_right_arm[N_ARM] = {
  90+60, 0, 0};

#define N_LEG 4
#define I_LEG_HIP 0
#define I_LEG_THIGH 1
#define I_LEG_KNEE 2
#define I_LEG_ANKLE 3

Servo servo_left_leg[N_LEG];
int ns_left_leg[N_LEG] = {
  20, 21, 22, 23};
int pos_left_leg[N_LEG] = {
  90, 90, 90, 90};

Servo servo_right_leg[N_LEG];
int ns_right_leg[N_LEG] = {
  24, 25, 26, 27};
int pos_right_leg[N_LEG] = {
  90, 90, 90, 90};


void setup()
{
  Serial.begin(9600);

  for(int i=0; i<N_HEAD; i++)
  {
    servo_head[i].attach(ns_head[i]);
  }

  for(int i=0; i<N_ARM; i++)
  {
    servo_left_arm[i].attach(ns_left_arm[i]);
    servo_right_arm[i].attach(ns_right_arm[i]);
  }

  for(int i=0; i<N_LEG; i++)
  {
    servo_left_leg[i].attach(ns_left_leg[i]);
    servo_right_leg[i].attach(ns_right_leg[i]);
  }

  set_position_stand();
  set_position_pump_high();
  update_servos();
}

void set_position_stand()
{
  pos_head[I_HEAD_NECK] = 90;

  pos_left_arm[I_ARM_SHOULDER] = 30;
  pos_left_arm[I_ARM_BICEP] = 180;
  pos_left_arm[I_ARM_ELBOW] = 180;

  pos_left_leg[I_LEG_HIP] = 90;
  pos_left_leg[I_LEG_THIGH] = 90;
  pos_left_leg[I_LEG_KNEE] = 90;
  pos_left_leg[I_LEG_ANKLE] = 90;

  pos_right_arm[I_ARM_SHOULDER] = 150;
  pos_right_arm[I_ARM_BICEP] = 0;
  pos_right_arm[I_ARM_ELBOW] = 0;

  pos_right_leg[I_LEG_HIP] = 90;
  pos_right_leg[I_LEG_THIGH] = 90;
  pos_right_leg[I_LEG_KNEE] = 90;
  pos_right_leg[I_LEG_ANKLE] = 90;
}

void set_position_waitfor()
{
  pos_head[I_HEAD_NECK] = 90;

  pos_left_arm[I_ARM_SHOULDER] = 30;
  pos_left_arm[I_ARM_BICEP] = 130;
  pos_left_arm[I_ARM_ELBOW] = 140;

  pos_left_leg[I_LEG_HIP] = 90;
  pos_left_leg[I_LEG_THIGH] = 115;
  pos_left_leg[I_LEG_KNEE] = 110;
  pos_left_leg[I_LEG_ANKLE] = 90;

  pos_right_arm[I_ARM_SHOULDER] = 150;
  pos_right_arm[I_ARM_BICEP] = 30;
  pos_right_arm[I_ARM_ELBOW] = 20;

  pos_right_leg[I_LEG_HIP] = 90;
  pos_right_leg[I_LEG_THIGH] = 65;
  pos_right_leg[I_LEG_KNEE] = 70;
  pos_right_leg[I_LEG_ANKLE] = 90;
}

void set_position_plane()
{
  pos_head[I_HEAD_NECK] = 90;

  pos_left_arm[I_ARM_SHOULDER] = 30;
  pos_left_arm[I_ARM_BICEP] = 90;
  pos_left_arm[I_ARM_ELBOW] = 180;

  pos_left_leg[I_LEG_HIP] = 0;
  pos_left_leg[I_LEG_THIGH] = 90;
  pos_left_leg[I_LEG_KNEE] = 90;
  pos_left_leg[I_LEG_ANKLE] = 180;

  pos_right_arm[I_ARM_SHOULDER] = 150;
  pos_right_arm[I_ARM_BICEP] = 90;
  pos_right_arm[I_ARM_ELBOW] = 0;

  pos_right_leg[I_LEG_HIP] = 180;
  pos_right_leg[I_LEG_THIGH] = 90;
  pos_right_leg[I_LEG_KNEE] = 90;
  pos_right_leg[I_LEG_ANKLE] = 0;
}

void set_position_pump_high()
{
  pos_head[I_HEAD_NECK] = 90;

  pos_left_arm[I_ARM_SHOULDER] = 110;
  pos_left_arm[I_ARM_BICEP] = 180;
  pos_left_arm[I_ARM_ELBOW] = 180;

  pos_left_leg[I_LEG_HIP] = 80;
  pos_left_leg[I_LEG_THIGH] = 90;
  pos_left_leg[I_LEG_KNEE] = 90;
  pos_left_leg[I_LEG_ANKLE] = 90;
  
  pos_right_arm[I_ARM_SHOULDER] = 60;
  pos_right_arm[I_ARM_BICEP] = 0;
  pos_right_arm[I_ARM_ELBOW] = 0;

  pos_right_leg[I_LEG_HIP] = 100;
  pos_right_leg[I_LEG_THIGH] = 90;
  pos_right_leg[I_LEG_KNEE] = 90;
  pos_right_leg[I_LEG_ANKLE] = 90;
}

void set_position_pump_low()
{
  set_position_pump_high();

  pos_left_arm[I_ARM_BICEP] = 130;
  pos_left_arm[I_ARM_ELBOW] = 90;

  pos_right_arm[I_ARM_BICEP] = 40;
  pos_right_arm[I_ARM_ELBOW] = 90;
}

void set_position_semipump_high()
{
  pos_head[I_HEAD_NECK] = 90;

  pos_left_arm[I_ARM_SHOULDER] = 90;
  pos_left_arm[I_ARM_BICEP] = 180;
  pos_left_arm[I_ARM_ELBOW] = 180;

  pos_left_leg[I_LEG_HIP] = 75;
  pos_left_leg[I_LEG_THIGH] = 90;
  pos_left_leg[I_LEG_KNEE] = 135;
  pos_left_leg[I_LEG_ANKLE] = 180;
  
  pos_right_arm[I_ARM_SHOULDER] = 90;
  pos_right_arm[I_ARM_BICEP] = 0;
  pos_right_arm[I_ARM_ELBOW] = 0;

  pos_right_leg[I_LEG_HIP] = 105;
  pos_right_leg[I_LEG_THIGH] = 90;
  pos_right_leg[I_LEG_KNEE] = 45;
  pos_right_leg[I_LEG_ANKLE] = 0;
}

void set_position_semipump_low()
{
  set_position_semipump_high();

  pos_left_arm[I_ARM_BICEP] = 130;
  pos_left_arm[I_ARM_ELBOW] = 90;

  pos_right_arm[I_ARM_BICEP] = 40;
  pos_right_arm[I_ARM_ELBOW] = 90;
}

void update_servos()
{
  for(int i=0; i<N_HEAD; i++)
  {
    servo_head[i].write(pos_head[i]);
  }

  for(int i=0; i<N_ARM; i++)
  {
    servo_left_arm[i].write(pos_left_arm[i]);
    servo_right_arm[i].write(pos_right_arm[i]);
  }

  for(int i=0; i<N_LEG; i++)
  {
    servo_left_leg[i].write(pos_left_leg[i]);
    servo_right_leg[i].write(pos_right_leg[i]);
  }

  delay(500);
}

void loop()
{
  /*
  char val = Serial.read();
  if(val>0)
  {
    switch(val)
    {
    case '0':
      set_position_stand();
      update_servos();
      break;
    case '1':
      set_position_waitfor();
      update_servos();
      break;
    case '4':
      set_position_pump_high();
      update_servos();
      break;
    case '5':
      set_position_pump_low();
      update_servos();
      break;
    case '7':
      set_position_plane();
      update_servos();
      break;
    }
  }*/
        delay(500);
        set_position_pump_high();
      update_servos();
      delay(500);
            set_position_pump_low();
      update_servos();
}

